#pragma once

#include <stdint.h>
#include <string>
#include <vector>
#include <cmath>


enum class FileType {
    FILE_TYPE_UNKNOWN = -1,          // Unknown
    FILE_TYPE_OTA = 0,               // OTA升级文件
    FILE_TYPE_ELEVATION = 1,         // 高程文件
    FILE_TYPE_FIXED_NO_FLYZONE = 2,  // 固定禁飞区文件
    FILE_TYPE_TEMP_NO_FLYZONE = 3,   // 临时禁飞区文件
    FILE_TYPE_AUTHORIZED_ZONE = 4,   // 授权区文件
    FILE_TYPE_ELECTRIC_BARRIER = 5,  // 电子围栏
    FILE_TYPE_WAYPOINT_MISSION = 6,  // 行点文件
    FILE_TYPE_ALBUM = 7,             // 相册文件
    FILE_TYPE_MISSION_AUDIO = 8,     // 喊话器文件
    FILE_TYPE_KML = 12,              // KML文件
    FILE_TYPE_FEASIBLE_REGION = 14,  // 可飞区文件
};

enum class OperResult {
    TIMEOUT = -1,  // 操作超时
    FAILED = 0, // 操作失败
    SUCCESS = 1,  // 操作失败
    ERROR = 2,  // 其他不明的错误
};

enum class MissionStatus {
    INIT = 0,  // 初始化
    RUNNING = 1,  // 运行中
    PAUSE = 2,  // 暂停
    STOP = 3,  // 停止
    FINISH = 4,  // 完成
};

// PRIORTY
enum class PriortyLevel {
    EVENT_NONE = 0,      // 正常事件
    EVENT_WARNING = 1,   // 异常事件
    WARN_WARNING = 1,    // 普通告警
    WARN_CTRITICAL = 2,  // 紧急告警
    WARN_EMERGENCY = 3,  // 严重告警
};

enum class ParamType {
    PARAM_TYPE_INT = 0,
    PARAM_TYPE_DOUBLE = 1,
    PARAM_TYPE_STRING = 2,
};

enum class ValidType {
    VALID_TYPE_FALSE = -1,  // 不校验
    VALID_TYPE_NONE = 1,  // 校验
    VALID_TYPE_TRUE = 1,  // 校验
};

struct HomePos_t {
    double home_lat{0};  // 起飞记录的gps的home位置
    double home_lon{0};
    float home_alt{0};
    float home_x{0};
    float home_y{0};
    float home_z{0};
    float home_yaw{0};          // radian
    bool home_valid{false};  // 是否可用
};
enum class ReasonId {
    NOTHING                 = 10000,
    LOW_BATTERY             ,           // change state for lower power
    EMER_BATTERY            ,          // change state for BATTERY_WARNING_EMERGENCY
    IN_NOFLYZONE            ,          // change state for enter no fly zone
    RC_LOSS                 ,               // change state for rc loss
    MISSION_FINISH          ,        // change state for mission finish
    CMD           ,         // change state for main_ctrl cmd
    ALREADY_SAME_MISSION    ,  // change state for already same mission
    GEOFENCE_ACTION         ,       // change state for geofence check
    UAV_ON_GROUND           ,
    PAUSE_BUTTON            = 10010,        // change state for push button of pause
    HOME_RETURN_BUTTON      ,  // home and return buttons
    TIME_NOT_SYNC           ,
    INTEL_LOW_BATTERY       ,  // change state for intel low battery
    FCS_ATTITUDE            ,       // 飞控姿态模式
    PLANNER_OVERTIME        ,   // planner的心跳超时
    MISSION_ERROR           ,      // 任务中间出错
    NET_LOSS                ,           // 4G断联
    RTK_FLOAT               ,          // rtk 浮点解
    MISSION_DIS_FAR_AWAY    ,    // 航线距离太远
    MISSION_POINTS_IN_NOFLY = 10020,        // 有航线穿过禁飞区
    MISSION_OUTSIDE_LIMITED ,        // 航线单点距离当前位置，很远
    NO_ODOM                 ,           // 起飞时没有飞控位置
    NO_GPS                  ,            // 起飞是没有gps
    MISSION_DIS_TOO_LONG    ,           // 整个航线作业距离过大
    MISSION_NO_FILE         ,                // 没有对应的任务文件
    MISSION_CMD_NO_ID       ,              // 开始指令非法，没有带id
    TAKEOFF_OUT_TIMEOUT     ,      // 起飞超时
    TAKEOFF_FINISHED        ,     // 起飞完成
};

enum EventId {
    // mission_manager have not data
    NO_ODOM = 0x1110,
    NO_GPS,
    NO_LOCAL_POS,
    NO_HOME,
    RC_LOSS,
};

enum class DroneType {
    TYPE_UNKNOWN = 0,  // unknown
    TYPE_4X4 = 1,       // 4x4
    TYPE_6X6 = 2,       // 6x6
};

enum class FlightControlMode {
    FLIGHT_CONTROL_MODE_UNKNOWN = 0,  // unknown
    FLIGHT_CONTROL_MODE_MANUAL = 1,   // manual
    FLIGHT_CONTROL_MODE_AUTO = 2,       // auto
    FLIGHT_CONTROL_MODE_OFFBOARD = 3,  // offboard
};

enum class CDateType {
    FCS_STATE,
    GPS_RAW,
    ODOM,
    CMD,
    CMD_ACK,
    HEARTBEAT_1HZ,
    HEARTBEAT_5HZ,
};

enum class CmdType {
    STOP = 0,  // stop
    LOITER = 1,  // loiter
    ARM = 2,   // arm可能用不上
    DISARM = 3,   // arm可能用不上
    TAKEOFF = 4, // takeoff
    LAND = 5,  // land
    RETURN_HOME = 6,  // return home
    WAYPOINT = 7,  // waypoint
};

struct InputCmd_t {
    int64_t timestamp{0};  // 时间戳
    int32_t cmd{0};  // 命令
    int32_t seq{0};  // 命令序号
    int32_t drone_type{(int32_t)DroneType::TYPE_4X4}; // 飞机类型 DroneType
    int32_t drone_id{0};  // 飞机id
    std::vector<double> d_params;  // 参数
    std::vector<int> i_params;  // int类型的参数
    std::vector<std::string> s_params;  // 字符串类型的参数

    void clear() {
        cmd = 0;
        d_params.clear();
        i_params.clear();
        s_params.clear();
    };
};

struct InputCmdAck_t {
    int64_t timestamp{0};  // 时间戳
    int32_t cmd{0};  // 命令
    int32_t seq{0};  // 命令序号
    int32_t drone_type{(int32_t)DroneType::TYPE_4X4}; // 飞机类型 DroneType
    int32_t drone_id{0};  // 飞机id
    uint8_t ack{0};  // 是否成功, 0失败, 1成功, 2 其他
};

struct HomePosition_s {
    enum HomeSource {
        HOME_SOURCE_UNKNOWN = 0,  // unknown
        HOME_SOURCE_RTK = 1,       // from RC
        HOME_SOURCE_GPS = 2,     // from GPS
        HOME_SOURCE_SLAM = 3,    // from SLAM
        HOME_SOURCE_MANUAL = 4,  // from manual
        HOME_SOURCE_NED = 5,     // from NED
        HOME_SOURCE_BACKUP0 = 6,  // from backup
        HOME_SOURCE_BACKUP1 = 7,  // from backup
        HOME_SOURCE_BACKUP2 = 8,   // from backup
    };
    HomeSource manual_home_source{};  // 当前自定义的返航点的来源， 0： unknown，1: 起飞点位置 2:
                                // 来自RC遥控器, 3: 来自飞机位置, 4: 备降点
    HomePos_t rtk_home_pos;
    HomePos_t gps_home_pos;
    HomePos_t slam_home_pos;
    HomePos_t manual_home_pos;
    HomePos_t ned_home_pos;
    HomePos_t backup_home_pos0;  // 备降点的位置
    HomePos_t backup_home_pos1;  // 备降点的位置
    HomePos_t backup_home_pos2;  // 备降点的位置
};

struct MissionHeartBeat1Hz_s {
    int64_t seq{0};
    uint8_t drone_type;   // 飞机类型 DroneType
    uint8_t drone_id;   // 飞机id
    uint8_t fcs_flight_mode;  // 飞行模式FlightControlMode
    bool fcs_arm;   // 

    uint8_t smart_mode;         // 飞机工作模式，主模式
    uint8_t smart_mode_status;  // 工作状态

    bool is_home_invalid;  // home点无效
    int32_t home_x_lat; // home点纬度 = home_x_lat * 1e-7
    int32_t home_y_lon; // home点经度 = home_y_lon * 1e-7
    int32_t home_z_alt; // home点高度 = home_z_alt * 1e-3
    int32_t backup_home_lat;             // 备降点0纬度 = backup_home_lat * 1e-7
    int32_t backup_home_lon;             // 备降点0经度 = backup_home_lon * 1e-7
    int32_t backup_home_alt;             // 备降点0高度 = backup_home_alt * 1e-3
    bool backup_home_valid;              // 备降点有效：TRUE

    int32_t fly_time_s;  // 飞行时间
    int32_t start_time_s; // 启动时间
};

struct MissionHeartBeat5Hz_s {
    int64_t seq{0};
    uint8_t drone_type;   // 飞机类型 DroneType
    uint8_t drone_id;   // 飞机id

    bool is_gps_invalid;  // home点无效
    int32_t uav_lat; // uav纬度 = uav_lat * 1e-7
    int32_t uav_lon; // home点经度 = home_y_lon * 1e-7
    int32_t uav_alt; // home点高度 = home_z_alt * 1e-3
    int32_t gps_num;  // 卫星数
    int32_t gps_fix;  // 定位状态

    int32_t roll;  // 滚转角
    int32_t pitch;  // 俯仰角
    int32_t yaw;  // 航向角
    int32_t position[3];  // 位置
    int32_t velocity[3];  // 速度
    int32_t distance_mm;  // 飞行距离m = distance_m * 1e-3
};

struct GpsRaw_s {
    enum GPS_FIX_STATUS{ 
        STATUS_NO_FIX=-1,
        STATUS_FIX=0,
        STATUS_SBAS_FIX=1,
        STATUS_GBAS_FIX=2,
    };
    enum SERVICE_TYPE{
        SERVICE_GPS=1,
        SERVICE_GLONASS=2,
        SERVICE_COMPASS=4,
        SERVICE_GALILEO=8,
    };
    int64_t timestamp{0};  // [us] timestamp{0} (UNIX Epoch time or time since system boot). The receiving end can infer timestamp{0} format (since 1.1.19
    GPS_FIX_STATUS fix_status;      // [GPS_FIX_STATUS] GPS fix type
    SERVICE_TYPE service;       // [SERVICE_TYPE] Service provider: 0-GPS, 1-GLONASS, 2-COMPASS, etc.

    double lat;         // [degE7] Latitude (WGS84, EGM96 ellipsoid)
    double lon;           // [degE7] Longitude (WGS84, EGM96 ellipsoid)
    double alt;           // [mm] Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude.
};

struct RtkGpsRaw_s {
    int64_t timestamp{0};      //  [us] Time since system start (microseconds).

    uint8_t rtk_receiver_id;      // Identification of connected RTK receiver.
    int16_t wn;                   // GPS Week Number of last baseline.
    uint32_t tow;                 // [ms] GPS Time of Week of last baseline.
    uint8_t rtk_health;           // GPS-specific health report for RTK data.
    uint8_t rtk_rate;             // [Hz] Rate of baseline messages being received by GPS.
    uint8_t nsats;                // Current number of sats used for RTK calculation.
    int32_t baseline_a;           // [mm] Current baseline in ECEF x or NED north component, depends on header.frame_id.
    int32_t baseline_b;           // [mm] Current baseline in ECEF y or NED east component, depends on header.frame_id.
    int32_t baseline_c;           // [mm] Current baseline in ECEF z or NED down component, depends on header.frame_id.
    uint32_t accuracy;            // Current estimate of baseline accuracy.
    int32_t iar_num_hypotheses;   // Current number of integer ambiguity hypotheses.
};

struct BatteryStatus_s
{
    /* data */
    int64_t timestamp{0};  // Message timestamp{0}, unit: nanoseconds
    float voltage;  // 电池电压
    float current;  // 电池电流
    float remaining;  // 电池剩余电量百分比[0,1]
};

struct FcsHomePosition_s {
    int64_t timestamp{0};
    double geo[3]; // geodetic coordinates in WGS-84 datum
    double position[3];	// local position
    float orientation[4];	// XXX: verify field name (q[4])
    float approach[3];	// position of the end of approach vector
};

struct ExtendedState_s
{
    enum VTOL_STATE{
        VTOL_STATE_UNDEFINED,
        VTOL_STATE_TRANSITION_TO_FW,
        VTOL_STATE_TRANSITION_TO_MC,
        VTOL_STATE_MC,
        VTOL_STATE_FW
    };
    enum LANDED_STATE{
        LANDED_STATE_UNDEFINED,
        LANDED_STATE_ON_GROUND,
        LANDED_STATE_IN_AIR,
        LANDED_STATE_TAKEOFF,
        LANDED_STATE_LANDING
    };
    int64_t timestamp{0};
    VTOL_STATE vtol_state;
    LANDED_STATE landed_state;
};

struct GlobalPositionTarget_s
{
    enum FRAMETYPE{
        FRAME_GLOBAL_INT = 5,
        FRAME_GLOBAL_REL_ALT = 6,
        FRAME_GLOBAL_TERRAIN_ALT = 11,
    };

    enum TYPEMASK{
        IGNORE_LATITUDE = 1,
        IGNORE_LONGITUDE = 2,
        IGNORE_ALTITUDE = 4,
        IGNORE_VX = 8,
        IGNORE_VY = 16,
        IGNORE_VZ = 32,
        IGNORE_AFX = 64,
        IGNORE_AFY = 128,
        IGNORE_AFZ = 256,
        FORCE = 512,
        IGNORE_YAW = 1024,
        IGNORE_YAW_RATE = 2048,
    };

    int64_t timestamp{0};
    uint8_t coordinate_frame;

    uint16_t type_mask;	// Bitmask to use when setting attitude. Each bit is the value of corresponding
    double latitude;
    double longitude;
    float altitude;	// in meters, AMSL or above terrain
    float velocity[3];
    float acceleration_or_force[3];
    float yaw;
    float yaw_rate;
};

struct PositionTarget_s
{
    int64_t timestamp{0};
    enum FRAMETYPE{
        FRAME_LOCAL_NED = 1,
        FRAME_LOCAL_OFFSET_NED = 7,
        FRAME_BODY_NED = 8,
        FRAME_BODY_OFFSET_NED = 9,
    };

    enum MODEMASK{
        IGNORE_PX = 1,   // Position ignore flags
        IGNORE_PY = 2,
        IGNORE_PZ = 4,
        IGNORE_VX = 8, // Velocity vector ignore flags
        IGNORE_VY = 16,
        IGNORE_VZ = 32,
        IGNORE_AFX = 64, // Acceleration/Force vector ignore flags
        IGNORE_AFY = 128,
        IGNORE_AFZ = 256,
        FORCE = 512, // Force in af vector flag
        IGNORE_YAW = 1024,
        IGNORE_YAW_RATE = 2048,
    };

    FRAMETYPE coordinate_frame;
    int type_mask;

    double position[3]; // 
    float velocity[3];
    float acceleration_or_force[3];
    float yaw;
    float yaw_rate;
};

struct State_s {
    int64_t timestamp{0};
    bool connected;
    bool armed{false};
    bool guided;
    bool manual_input;
    std::string mode; // 模式,见FcsMode_s
    uint8_t system_status;
};

struct FcsMode_s
{
    std::string MODE_APM_PLANE_MANUAL       = "MANUAL";
    std::string MODE_APM_PLANE_CIRCLE       = "CIRCLE";
    std::string MODE_APM_PLANE_STABILIZE    = "STABILIZE";
    std::string MODE_APM_PLANE_TRAINING     = "TRAINING";
    std::string MODE_APM_PLANE_ACRO         = "ACRO";
    std::string MODE_APM_PLANE_FBWA         = "FBWA";
    std::string MODE_APM_PLANE_FBWB         = "FBWB";
    std::string MODE_APM_PLANE_CRUISE       = "CRUISE";
    std::string MODE_APM_PLANE_AUTOTUNE     = "AUTOTUNE";
    std::string MODE_APM_PLANE_AUTO         = "AUTO";
    std::string MODE_APM_PLANE_RTL          = "RTL";
    std::string MODE_APM_PLANE_LOITER       = "LOITER";
    std::string MODE_APM_PLANE_LAND         = "LAND";
    std::string MODE_APM_PLANE_GUIDED       = "GUIDED";
    std::string MODE_APM_PLANE_INITIALISING = "INITIALISING";
    std::string MODE_APM_PLANE_QSTABILIZE   = "QSTABILIZE";
    std::string MODE_APM_PLANE_QHOVER       = "QHOVER";
    std::string MODE_APM_PLANE_QLOITER      = "QLOITER";
    std::string MODE_APM_PLANE_QLAND        = "QLAND";
    std::string MODE_APM_PLANE_QRTL         = "QRTL";

    std::string MODE_APM_COPTER_STABILIZE   = "STABILIZE";
    std::string MODE_APM_COPTER_ACRO        = "ACRO";
    std::string MODE_APM_COPTER_ALT_HOLD    = "ALT_HOLD";
    std::string MODE_APM_COPTER_AUTO        = "AUTO";
    std::string MODE_APM_COPTER_GUIDED      = "GUIDED";
    std::string MODE_APM_COPTER_LOITER      = "LOITER";
    std::string MODE_APM_COPTER_RTL         = "RTL";
    std::string MODE_APM_COPTER_CIRCLE      = "CIRCLE";
    std::string MODE_APM_COPTER_POSITION    = "POSITION";
    std::string MODE_APM_COPTER_LAND        = "LAND";
    std::string MODE_APM_COPTER_OF_LOITER   = "OF_LOITER";
    std::string MODE_APM_COPTER_DRIFT       = "DRIFT";
    std::string MODE_APM_COPTER_SPORT       = "SPORT";
    std::string MODE_APM_COPTER_FLIP        = "FLIP";
    std::string MODE_APM_COPTER_AUTOTUNE    = "AUTOTUNE";
    std::string MODE_APM_COPTER_POSHOLD     = "POSHOLD";
    std::string MODE_APM_COPTER_BRAKE       = "BRAKE";
    std::string MODE_APM_COPTER_THROW       = "THROW";
    std::string MODE_APM_COPTER_AVOID_ADSB  = "AVOID_ADSB";
    std::string MODE_APM_COPTER_GUIDED_NOGPS= "GUIDED_NOGPS";

    std::string MODE_APM_ROVER_MANUAL       = "MANUAL";
    std::string MODE_APM_ROVER_LEARNING     = "LEARNING";
    std::string MODE_APM_ROVER_STEERING     = "STEERING";
    std::string MODE_APM_ROVER_HOLD         = "HOLD";
    std::string MODE_APM_ROVER_AUTO         = "AUTO";
    std::string MODE_APM_ROVER_RTL          = "RTL";
    std::string MODE_APM_ROVER_GUIDED       = "GUIDED";
    std::string MODE_APM_ROVER_INITIALISING = "INITIALISING";

    std::string MODE_PX4_MANUAL             = "MANUAL";
    std::string MODE_PX4_ACRO               = "ACRO";
    std::string MODE_PX4_ALTITUDE           = "ALTCTL";
    std::string MODE_PX4_POSITION           = "POSCTL";
    std::string MODE_PX4_OFFBOARD           = "OFFBOARD";
    std::string MODE_PX4_STABILIZED         = "STABILIZED";
    std::string MODE_PX4_RATTITUDE          = "RATTITUDE";
    std::string MODE_PX4_MISSION            = "AUTO.MISSION";
    std::string MODE_PX4_LOITER             = "AUTO.LOITER";
    std::string MODE_PX4_RTL                = "AUTO.RTL";
    std::string MODE_PX4_LAND               = "AUTO.LAND";
    std::string MODE_PX4_RTGS               = "AUTO.RTGS";
    std::string MODE_PX4_READY              = "AUTO.READY";
    std::string MODE_PX4_TAKEOFF            = "AUTO.TAKEOFF";
};

struct Odometry_s
{
    int64_t timestamp{0};
    std::string frame_id;
    std::string child_frame_id;
    double pos[3];
    double rpy[3];
    double vel_linear[3];
    double vel_angular[6];
    double velXy(){
        return std::sqrt(vel_linear[0]*vel_linear[0]+vel_linear[1]*vel_linear[1]);
    };
    double velLen(){
        return std::sqrt(vel_linear[0]*vel_linear[0]+vel_linear[1]*vel_linear[1]+vel_linear[2]*vel_linear[2]);
    };

    double distance(){
        return std::sqrt(pos[0]*pos[0]+pos[1]*pos[1]);
    };
};








